#include <stdio.h>
#include <stdbool.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <util/crc16.h>
#include "modbus.h"
#include "timer.h"

#define BAUD MODBUS_SPEED
#include <util/setbaud.h>

static uint8_t slave_addr;
static MODBUS_HANDLER command_handler;


void modbus_init(uint8_t address, MODBUS_HANDLER handler)
{
    slave_addr = address;
    command_handler = handler;
    
    MODBUS_DIR_DDR |= _BV(MODBUS_DIR);
    MODBUS_DIR_PORT &= ~_BV(MODBUS_DIR);
    PORTD |= _BV(PD0); // pull up RXD

#if defined (UCSRA)
    #if USE_2X
    UART_STATUS = _BV(U2X);
    #else
    UART_STATUS = 0;
    #endif
    UBRRH = UBRRH_VALUE;
    UBRRL = UBRRL_VALUE;
    UCSRB = _BV(RXCIE) | _BV(RXEN) | _BV(TXEN);
    UCSRC = _BV(URSEL) | _BV(UPM1) | _BV(UCSZ1) | _BV(UCSZ0);
#elif defined (UCSR0A)
    #if USE_2X
    UART_STATUS = _BV(U2X0);
    #else
    UART_STATUS = 0;
    #endif
    UBRR0H = UBRRH_VALUE;
    UBRR0L = UBRRL_VALUE;
    UCSR0B = _BV(RXCIE0) | _BV(RXEN0) | _BV(TXEN0);
    UCSR0C = _BV(UPM01) | _BV(UCSZ01) | _BV(USCZ00);
#endif
}


__attribute__ ((noinline))
static void modbus_crc(uint16_t *crc, uint8_t c)
{
    *crc = _crc16_update(*crc, c);
}


static void modbus_receive(bool data_received, uint8_t c)
{
    static uint16_t timer;
    static uint8_t data[MODBUS_PDU_LEN];
    static uint8_t ptr = 0;
    static uint16_t crc = 0xffff;
    
    if (data_received) {
        data[ptr++] = c;
        if (ptr >= MODBUS_PDU_LEN) ptr = MODBUS_PDU_LEN - 1;
        modbus_crc(&crc, c);
        ResetDelay(&timer);
    }
    else {
        // timeout check
        if (HasDelayElapsed((uint16_t)MODBUS_TIMEOUT, &timer, false)) {
            // data length check
            if (ptr >= 4) {
                // address check
                if (data[0] == 0 || data[0] == slave_addr || data[0] == 248) {
                    // CRC checking
                    if ((data[ptr-2] == (crc & 0x00ff) && (data[ptr-1] == (crc & 0xff00) >> 8))) {
                        // handle DIAGNOSTIC command
                        if (data[1] == MODBUS_DIAGNOSTIC) {

                        }
                        // try to handle the command
                        else if (!command_handler(data[1], &data[2], ptr-4, data[0] ? false : true)) {
                            // if not handled and not broadcasted, return error
                            if (data[0]) modbus_send(data[1] | MODBUS_ERROR, (uint8_t[]){MODBUS_ERR_ILLEGAL_FUNCTION}, 1);
                        }
                    }
                }
            }
            ptr = 0;
            crc = 0xffff;
        }
    }
}


ISR(USART_RXC_vect)
{
    modbus_receive(true, UART_DATA);
}


void modbus_task(void)
{
    modbus_receive(false, 0);
}


void modbus_send(uint8_t function, uint8_t *data, uint8_t length)
{
    uint16_t crc = 0xffff;

    MODBUS_DIR_PORT |= _BV(MODBUS_DIR);
    _delay_us(MODBUS_DIR_SETUP);

    UART_STATUS |= _BV(TXC);
    UART_DATA = slave_addr;
    modbus_crc(&crc, slave_addr);
    loop_until_bit_is_set(UART_STATUS, TXC);
    
    UART_STATUS |= _BV(TXC);
    UART_DATA = function;
    modbus_crc(&crc, function);
    loop_until_bit_is_set(UART_STATUS, TXC);

    while (length--) {
        UART_STATUS |= _BV(TXC);
        UART_DATA = *data;
        modbus_crc(&crc, *data);
        data++;
        loop_until_bit_is_set(UART_STATUS, TXC);
    }

    UART_STATUS |= _BV(TXC);
    UART_DATA = (crc & 0x00ff);
    loop_until_bit_is_set(UART_STATUS, TXC);

    UART_STATUS |= _BV(TXC);
    UART_DATA = (crc & 0xff00) >> 8;
    loop_until_bit_is_set(UART_STATUS, TXC);

    _delay_us(MODBUS_DIR_HOLD);
    MODBUS_DIR_PORT &= ~_BV(MODBUS_DIR);
}

